#include <iostream>
#include "publish_pointcloud.hpp"
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/filters/filter.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/registration/gicp.h>
#include <pcl/features/fpfh.h>
#include <pcl/registration/ia_ransac.h>
#include <ceres/ceres.h>
#include <sophus/se3.hpp> 
#include <Eigen/Dense>
#include <pcl/registration/ndt.h>
#include <pcl/filters/approximate_voxel_grid.h>


// 检查点是否在光线路径上
bool isPointOnRay(const pcl::PointXYZ& point, const Ray& ray, float tolerance) {
    Eigen::Vector3f point_vec(point.x, point.y, point.z);
    Eigen::Vector3f to_point = point_vec - ray.origin;
    float dot_product = to_point.dot(ray.direction);
    if (dot_product < 0) return false;  // 点在光线反方向
    Eigen::Vector3f projected_point = ray.origin + dot_product * ray.direction;
    Eigen::Vector3f diff = point_vec - projected_point;
    return diff.norm() < tolerance;
}

void cut_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud,pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud)
{
    std::size_t size;
    pcl::PointXYZ p_temp;
    size=input_cloud->points.size();
    for(int i=0;i<size;i++)
    {
        if(input_cloud->points[i].y>0)
        {
            p_temp.x=input_cloud->points[i].x;
            p_temp.z=input_cloud->points[i].z*(-1.0)+1.1;
            p_temp.y=input_cloud->points[i].y;
            if((p_temp.y<4.3)&(p_temp.y>0.73)&(p_temp.z>0.25)&(p_temp.x<1.0)&(p_temp.x>-1.0))
            output_cloud->points.push_back(p_temp);
        }
    }
    pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;
    voxel_grid.setInputCloud(output_cloud);
    voxel_grid.setLeafSize(0.01f, 0.01f, 0.01f); // 设置体素大小为0.01米
    voxel_grid.filter(*output_cloud);
     // 半径滤波 
    //  pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem; 
    //  outrem.setInputCloud(output_cloud);  
    //  outrem.setRadiusSearch(0.3);  
    //  outrem.setMinNeighborsInRadius(3);  
    //  outrem.filter(*output_cloud);  
//rayBasedDenoising(output_cloud);
}

 
void rayBasedDenoising(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    kdtree.setInputCloud(cloud); 
    
    std::vector<int> valid_indices;
    
    for (size_t i = 0; i < cloud->size(); ++i) {
        pcl::PointXYZ pt = cloud->points[i];
        Eigen::Vector3f direction(pt.x, pt.y, pt.z);
        direction.normalize();  // 获取归一化方向向量 
        
        // 沿光线方向搜索最近点
        std::vector<int> indices;
        std::vector<float> distances;
        if (kdtree.radiusSearch(pt,  0.1, indices, distances) > 0) { // 搜索半径需根据点间距调整 
            bool is_occluded = false;
            for (auto& idx : indices) {
                if (idx == i) continue;
                Eigen::Vector3f neighbor_pt = cloud->points[idx].getVector3fMap();
                // 判断点是否在同一直线且更靠近原点 
                if (neighbor_pt.normalized().dot(direction)  > 0.99 && 
                    neighbor_pt.norm()  < pt.getVector3fMap().norm())  {
                    is_occluded = true;
                    break;
                }
            }
            if (!is_occluded) valid_indices.push_back(i); 
        }
    }
    
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::copyPointCloud(*cloud, valid_indices, *filtered_cloud);
}



void computeTransform(pcl::PointCloud<pcl::PointXYZ>::Ptr source,pcl::PointCloud<pcl::PointXYZ>::Ptr target )
   {
    float dx, dy;
    float theta_z;

    // pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    // pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
    // sor.setInputCloud(cloud); 
    // sor.setMeanK(5);        // 统计邻域点数 
    // sor.setStddevMulThresh(1.0);  // 标准差倍数阈值 
    // pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    // sor.filter(*cloud_filtered); 
    //filter_point(target);
    
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    icp.setInputSource(source); 
    icp.setInputTarget(target); 
  
  // 设置ICP参数 
    icp.setMaximumIterations(300);   // 最大迭代次数 
    icp.setTransformationEpsilon(1e-9);  // 收敛精度 
    icp.setMaxCorrespondenceDistance(0.08);  // 最大对应点距离 
  
  // 执行配准 
    pcl::PointCloud<pcl::PointXYZ> final_cloud;
    icp.align(final_cloud); 
  
  // 获取变换矩阵 
     Eigen::Matrix4f transform = icp.getFinalTransformation(); 
  
  // 提取平移分量 
     dx = transform(0,3);  // X轴平移 
    dy = transform(1,3);  // Y轴平移 
  
  // 计算Z轴旋转角度（弧度转角度）
     theta_z = atan2(transform(1,0), transform(0,0)) * 180.0 /3.1415927;
    std::cout<<dx<<" "<<dy<<"  "<<theta_z<<std::endl;
}
void filter_point(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
   //pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
    sor.setInputCloud(cloud); 
    sor.setMeanK(3);        // 统计邻域点数 
    sor.setStddevMulThresh(0.08);  // 标准差倍数阈值 
   // pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    sor.filter(*cloud); 
}
void comput3Dcenter(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,float &x,float &y)
{
    Eigen::Vector4f centroid;
    pcl::compute3DCentroid(*cloud, centroid); 
    x=centroid[0];
    y=centroid[1];
}
void shift_point(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,float x,float y)
{
    std::size_t size;
    size=cloud->points.size();
    for(int i=0;i<size;++i)
    {
        cloud->points[i].x=cloud->points[i].x+x;
        cloud->points[i].y=cloud->points[i].y+y;
    }
}

///GICP 点云配准
void gicp_compare(pcl::PointCloud<pcl::PointXYZ>::Ptr source,pcl::PointCloud<pcl::PointXYZ>::Ptr target )
{
    
    // 2. 预处理（降采样+去噪）
    pcl::VoxelGrid<pcl::PointXYZ> voxel;
    voxel.setLeafSize(0.01,  0.01, 0.01); // 体素尺寸0.01m
    voxel.setInputCloud(source); 
    voxel.filter(*source); 
    voxel.setInputCloud(target); 
    voxel.filter(*target); 
 
    // 3. 初始化GICP配准器
    pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> gicp;
    gicp.setInputSource(source); 
    gicp.setInputTarget(target); 
    
    // 4. 参数设置 
    gicp.setRotationEpsilon(1e-3);     // 旋转收敛阈值 
    gicp.setCorrespondenceRandomness(20);  // 增加随机采样数
    gicp.setMaximumOptimizerIterations(50);  // 优化器迭代次数

    gicp.setMaximumIterations(300);       // 最大迭代次数 
    gicp.setTransformationEpsilon(1e-8);  // 变换矩阵收敛阈值 
    gicp.setMaxCorrespondenceDistance(0.03);  // 最大对应点距离 
    gicp.setRANSACIterations(30);         // RANSAC迭代次数 
 
    // 5. 执行配准 
    pcl::PointCloud<pcl::PointXYZ> final_cloud;
    gicp.align(final_cloud); 
 
    // 6. 输出结果 
    if (gicp.hasConverged())  {
       // std::cout << "变换矩阵:\n" << gicp.getFinalTransformation()  << std::endl;
        // 获取变换矩阵 
        Eigen::Matrix4f transform = gicp.getFinalTransformation(); 
        float dx,dy,theta_z;
        // 提取平移分量 
         dx = transform(0,3);  // X轴平移 
         dy = transform(1,3);  // Y轴平移 
         theta_z = atan2(transform(1,0), transform(0,0)) * 180 /3.1415927;
         std::cout<<dx<<" "<<dy<<"  "<<theta_z<<std::endl;
        std::cout << "Fitness Score: " << gicp.getFitnessScore()  << std::endl;
       // io::savePCDFile("result.pcd",  final_cloud);
    } else {
        std::cerr << "配准失败" << std::endl;
    }   
}

void pipei_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_src,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tgt)
{
    float dx,dy,theta_z;
    // 2. 预处理（降采样+去噪）
    pcl::VoxelGrid<pcl::PointXYZ> vg;
    vg.setLeafSize(0.02f,  0.02f, 0.02f); // 体素尺寸根据需求调整 
    vg.setInputCloud(cloud_src); 
    vg.filter(*cloud_src); 
    std::cout<<"this1="<<cloud_src->points.size()<<std::endl;
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
    sor.setMeanK(5);          // 邻域点数 
    sor.setStddevMulThresh(1.0);  // 标准差倍数阈值 
    sor.setInputCloud(cloud_tgt); 
    sor.filter(*cloud_tgt); 
    std::cout<<"this2="<<cloud_tgt->points.size()<<std::endl;
    // 3. 法线估计（FPFH特征必需）
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
    pcl::PointCloud<pcl::Normal>::Ptr normals_in (new pcl::PointCloud<pcl::Normal>); 
    pcl::PointCloud<pcl::Normal>::Ptr normals_out (new pcl::PointCloud<pcl::Normal>); 
    
    ne.setInputCloud(cloud_src); 
    ne.setSearchMethod(tree); 
    ne.setRadiusSearch(0.1);   // 搜索半径根据点密度调整 
    ne.compute(*normals_in); 
    ne.setInputCloud(cloud_tgt);
    ne.compute(*normals_out); 


    //std::cout<<"this2="<<normals_src->size()<<std::endl;
      // 4. FPFH特征提取 
      pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh;
      pcl::PointCloud<pcl::FPFHSignature33>::Ptr features_in(new pcl::PointCloud<pcl::FPFHSignature33>);
      pcl::PointCloud<pcl::FPFHSignature33>::Ptr features_out(new pcl::PointCloud<pcl::FPFHSignature33>);
      fpfh.setInputCloud(cloud_src); 
      fpfh.setInputNormals(normals_in); 
      fpfh.setSearchMethod(tree); 
      fpfh.setRadiusSearch(0.3);  // 特征提取半径 
      fpfh.compute(*features_in); 

      fpfh.setInputCloud(cloud_tgt); 
      fpfh.setInputNormals(normals_out); 
      fpfh.setSearchMethod(tree); 
      fpfh.setRadiusSearch(0.3);  // 特征提取半径 
      fpfh.compute(*features_out);  

    //  std::cout<<"this3="<<features_src->size()<<std::endl;
 //std::cout<<"here1"<<std::endl;
      // 5. 粗配准（基于FPFH的SAC-IA算法）
      pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> sac_ia;
    sac_ia.setInputSource(cloud_src); 
    sac_ia.setSourceFeatures(features_in); 
    sac_ia.setInputTarget(cloud_tgt); 
    sac_ia.setTargetFeatures  (features_out); // 确保设置目标特征
    sac_ia.setMinSampleDistance(0.03); 
    sac_ia.setNumberOfSamples(10);     // 采样点数 
    sac_ia.setCorrespondenceRandomness(5);  // 特征随机性 

    pcl::PointCloud<pcl::PointXYZ>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZ>);
    sac_ia.align(*aligned); 
    Eigen::Matrix4f initial_transform = sac_ia.getFinalTransformation(); 
    std::cout<<"here2"<<std::endl;
     // 6. 精配准（ICP算法）
     pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
     icp.setInputSource(aligned); 
     icp.setInputTarget(cloud_tgt); 
     icp.setMaximumIterations(100);    // 最大迭代次数 
     icp.setTransformationEpsilon(1e-8);  // 收敛阈值 
     icp.setEuclideanFitnessEpsilon(1e-6); 

     pcl::PointCloud<pcl::PointXYZ> Final;
     icp.align(Final); 
    
     // 6. 输出结果 
    if (icp.hasConverged())  {
        // std::cout << "变换矩阵:\n" << gicp.getFinalTransformation()  << std::endl;
         // 获取变换矩阵 
         Eigen::Matrix4f transform =icp.getFinalTransformation(); 
         //initial_transform
         dx = initial_transform(0,3);  // X轴平移 
         dy = initial_transform(1,3);  // Y轴平移 
         theta_z = atan2(initial_transform(1,0), initial_transform(0,0)) * 180 /3.1415927;
         std::cout<<dx<<" "<<dy<<"  "<<theta_z<<std::endl;
        //std::cout << "Fitness Score: " << icp.getFitnessScore()  << std::endl;

         // 提取平移分量 
          dx = transform(0,3);  // X轴平移 
          dy = transform(1,3);  // Y轴平移 
          theta_z = atan2(transform(1,0), transform(0,0)) * 180 /3.1415927;
          std::cout<<dx<<" "<<dy<<"  "<<theta_z<<std::endl;
         std::cout << "Fitness Score: " << icp.getFitnessScore()  << std::endl;
        // io::savePCDFile("result.pcd",  final_cloud);
     } else {
         std::cerr << "配准失败" << std::endl;
     }   


}
void ggicp(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target)
{
    // 将点云转换为带有法线的点云类型
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source_xyz(new pcl::PointCloud<pcl::PointXYZ>(*cloud_source));
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target_xyz(new pcl::PointCloud<pcl::PointXYZ>(*cloud_target));

    // 计算法线
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_source(new pcl::PointCloud<pcl::Normal>);
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_target(new pcl::PointCloud<pcl::Normal>);

    ne.setInputCloud(cloud_source_xyz);
    ne.setSearchMethod(tree);
    ne.setKSearch(50);
    ne.compute(*cloud_normals_source);

    ne.setInputCloud(cloud_target_xyz);
    ne.setSearchMethod(tree);
    ne.setKSearch(50);
    ne.compute(*cloud_normals_target);

    // 创建带法线的点云
    pcl::PointCloud<pcl::PointNormal>::Ptr cloud_source_with_normals(new pcl::PointCloud<pcl::PointNormal>);
    pcl::PointCloud<pcl::PointNormal>::Ptr cloud_target_with_normals(new pcl::PointCloud<pcl::PointNormal>);

    pcl::concatenateFields(*cloud_source_xyz, *cloud_normals_source, *cloud_source_with_normals);
    pcl::concatenateFields(*cloud_target_xyz, *cloud_normals_target, *cloud_target_with_normals);

    // GICP初始化
    pcl::GeneralizedIterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> gicp;
    gicp.setInputSource(cloud_source_with_normals);
    gicp.setInputTarget(cloud_target_with_normals);

    // 设置GICP参数
    gicp.setMaximumIterations(100);  // 最大迭代次数
    gicp.setTransformationEpsilon(1e-6);  // 收敛阈值
    gicp.setMaxCorrespondenceDistance(0.05);  // 对应点最大距离

    // 执行GICP配准
    pcl::PointCloud<pcl::PointNormal> Final;
    gicp.align(Final);

    if (gicp.hasConverged()) {
        std::cout << "GICP converged with score: " << gicp.getFitnessScore() << std::endl;

        Eigen::Matrix4f transformation = gicp.getFinalTransformation();
        std::cout << "Transformation Matrix:\n" << transformation << std::endl;

        // 提取平移向量
        Eigen::Vector3f translation = transformation.block<3,1>(0,3);
        std::cout << "Translation (x, y, z): " << translation.transpose() << std::endl;

        // 计算绕Z轴的旋转角
        double angle_z = atan2(transformation(1,0), transformation(0,0));
        std::cout << "Rotation around Z axis: " << angle_z * 180 / M_PI << " degrees" << std::endl;
    } else {
        PCL_ERROR("GICP did not converge.\n");
    }

}


void ndt_test(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target)
{
    // 使用体素网格滤波器减少点云密度
    pcl::ApproximateVoxelGrid<pcl::PointXYZ> voxel_filter;
    voxel_filter.setLeafSize(0.01, 0.01, 0.01);  // 设置体素大小
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud_source(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud_target(new pcl::PointCloud<pcl::PointXYZ>);
    voxel_filter.setInputCloud(cloud_source);
    voxel_filter.filter(*filtered_cloud_source);
    voxel_filter.setInputCloud(cloud_target);
    voxel_filter.filter(*filtered_cloud_target);

    // NDT初始化
    pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
    ndt.setInputSource(filtered_cloud_source);
    ndt.setInputTarget(filtered_cloud_target);

    // 设置NDT参数
    ndt.setTransformationEpsilon(0.01);  // 收敛阈值
    ndt.setStepSize(0.02);  // 优化步长
    ndt.setResolution(0.05);  // 网格分辨率
    ndt.setMaximumIterations(300);  // 最大迭代次数

    // 执行NDT配准
    pcl::PointCloud<pcl::PointXYZ> Final;
    Eigen::Matrix4f init_guess = Eigen::Matrix4f::Identity();  // 初始猜测（单位矩阵）
    ndt.align(Final, init_guess);

    if (ndt.hasConverged()) {
        std::cout << "NDT converged with score: " << ndt.getFitnessScore() << std::endl;

        Eigen::Matrix4f transformation = ndt.getFinalTransformation();
        std::cout << "Transformation Matrix:\n" << transformation << std::endl;

        // 提取平移向量
        Eigen::Vector3f translation = transformation.block<3,1>(0,3);
        std::cout << "Translation (x, y, z): " << translation.transpose() << std::endl;

        // 计算绕Z轴的旋转角
        double angle_z = atan2(transformation(1,0), transformation(0,0));
        std::cout << "Rotation around Z axis: " << angle_z * 180 / M_PI << " degrees" << std::endl;
    } else {
        PCL_ERROR("NDT did not converge.\n");
    }
}